
public class Wahadlo {
	
	double a[],b[],c[],d[]; // parametry obliczeniowe
	double x0,y0,x1,y1,x2,y2; //polozenia pktu zaczepienia i kulek
	double w1,w2,teta1,teta2; //pr. i pol. katowe
	double m1,m2,l1,l2; //masy kulek, dl. linek
	double g,h; //przysp. ziemskie, kwant czasu
	
	public Wahadlo(double w1,double w2,double teta1,double teta2,double l1,double l2,double m1,double m2,double x0, double y0){
		a = new double[4];
		b = new double[4];
		c = new double[4];
		d = new double[4];
		this.w1=w1;
		this.w2=w2;
		this.teta1=teta1;
		this.teta2=teta2;
		this.m1=m1;
		this.m2=m2;
		this.l1=l1;
		this.l2=l2;
		this.x0=x0;
		this.y0=y0;
		g=-9.81;
		h=0.05;
	}
	
	void wyliczPolozenia () {
		x1=l1*Math.sin(teta1)+x0;
		y1=-l1*Math.cos(teta1)+y0;
		x2= x1+l2*Math.sin(teta2) ;
		y2=+y1-l2*Math.cos(teta2) ;
	}
	
	void nastKrok () {	
		teta1 += h*(a[0]+2*b[0]+2*c[0]+d[0])/6;
		teta2 += h*(a[1]+2*b[1]+2*c[1]+d[1])/6;
		w1 += h*(a[2]+2*b[2]+2*c[2]+d[2])/6;
		w2 += h*(a[3]+2*b[3]+2*c[3]+d[3])/6;
	}
	
	void A() { 
		a[0] = w1;
		a[1] = w2;
		a[2] = (-g*(2*m1+m2)*Math.sin(teta1)-m2*g*Math.sin(teta1-2*teta2)-2*Math.sin(teta1-teta2)*m2*(w2*w2*l2+w1*w1*l1*Math.cos(teta1-teta2)))/(l1*(2*m1+m2-m2*Math.cos(2*teta1-2*teta2)));
		a[3] = (2*Math.sin(teta1-teta2)*(w1*w1*l1*(m1+m2)+g*(m1+m2)*Math.cos(teta1)+w2*w2*l2*m2*Math.cos(teta1-teta2)))/(l2*(2*m1+m2-m2*Math.cos(2*teta1-2*teta2)));
	}

	void B() { 
		double[] tmp = new double[4];
		tmp[0]=teta1; tmp[1]=teta2; tmp[2] = w1; tmp[3] = w2;
		for (int i=0; i<4; i++) tmp[i] += 0.5*h*a[i];
		b[0] = tmp[2];
		b[1] = tmp[3];
		b[2] = (-g*(2*m1+m2)*Math.sin(tmp[0])-m2*g*Math.sin(tmp[0]-2*tmp[1])-2*Math.sin(tmp[0]-tmp[1])*m2*(tmp[3]*tmp[3]*l2+tmp[2]*tmp[2]*l1*Math.cos(tmp[0]-tmp[1])))/(l1*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
		b[3] = (2*Math.sin(tmp[0]-tmp[1])*(tmp[2]*tmp[2]*l1*(m1+m2)+g*(m1+m2)*Math.cos(tmp[0])+tmp[3]*tmp[3]*l2*m2*Math.cos(tmp[0]-tmp[1])))/(l2*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
	}

	void C() { 
		double[] tmp = new double[4];
		tmp[0]=teta1; tmp[1]=teta2; tmp[2] = w1; tmp[3] = w2;
		for (int i=0; i<4; i++) tmp[i] += 0.5*h*b[i];
		c[0] = tmp[2];
		c[1] = tmp[3];
		c[2] = (-g*(2*m1+m2)*Math.sin(tmp[0])-m2*g*Math.sin(tmp[0]-2*tmp[1])-2*Math.sin(tmp[0]-tmp[1])*m2*(tmp[3]*tmp[3]*l2+tmp[2]*tmp[2]*l1*Math.cos(tmp[0]-tmp[1])))/(l1*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
		c[3] = (2*Math.sin(tmp[0]-tmp[1])*(tmp[2]*tmp[2]*l1*(m1+m2)+g*(m1+m2)*Math.cos(tmp[0])+tmp[3]*tmp[3]*l2*m2*Math.cos(tmp[0]-tmp[1])))/(l2*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
	}

	void D() { 
		double[] tmp = new double[4];
		tmp[0]=teta1; tmp[1]=teta2; tmp[2] = w1; tmp[3] = w2;
		for (int i=0; i<4; i++) tmp[i] += h*c[i];
		d[0] = tmp[2];
		d[1] = tmp[3];
		d[2] = (-g*(2*m1+m2)*Math.sin(tmp[0])-m2*g*Math.sin(tmp[0]-2*tmp[1])-2*Math.sin(tmp[0]-tmp[1])*m2*(tmp[3]*tmp[3]*l2+tmp[2]*tmp[2]*l1*Math.cos(tmp[0]-tmp[1])))/(l1*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
		d[3] = (2*Math.sin(tmp[0]-tmp[1])*(tmp[2]*tmp[2]*l1*(m1+m2)+g*(m1+m2)*Math.cos(tmp[0])+tmp[3]*tmp[3]*l2*m2*Math.cos(tmp[0]-tmp[1])))/(l2*(2*m1+m2-m2*Math.cos(2*tmp[0]-2*tmp[1])));
	}
}
